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ABSTRACT 

In this paper, we present an improved path planning method and obstacle avoidance algorithm for a 2-wheel 
mobile robot. Firstly, we briefly introduce the rapidly exploring random tree (RRT) and the single polar polynomial (SPP) 
algorithm. Secondly, we present additional 2 methods for applying our proposed method. Thirdly, we propose a global path 
planning, smoothing and obstacle avoidance method that combine RRT and SPP algorithm. Finally, we present simulation 
using proposed method and check the feasibility. This shows that proposed method is better than existing methods in terms 
of the optimality of the trajectory and the satisfaction of the kinematic constraints. 
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INTRODUCTION 

Background/ Objectives and Goals 

Recently, an autonomous robot like a drone and a self-driving car is an important issue. The autonomous robot 
means a robot which has high autonomy and can make decision and action by itself in a dynamic environment. Path 
planning is an important part in autonomous robots because it is a kind of criterion which tells how robot’s autonomy is 
good. 

Generating the shortest path is the most important part in the path planning algorithm. There are several path 
planning algorithms using graph or tree structure which are Dijkstra (Thomas H. Cormen et al., 2001), A* (W.ZENG and 
R. L. CHURCH, 2009) (Delling, D. et al., 2009), and D* (Stentz Anthony, 1994) (Stentz Anthony, 1995). These 
algorithms divide maps into many nodes, and find the shortest path according to each of their methods. Those algorithms 
have an advantage that they can get the optimal shortest path. However, those algorithms have a disadvantage that they are 
inefficient because it may explore all nodes in the high-dimensional configuration environment. 

Because of those reasons, several randomized algorithms have been proposed and successfully applied to the 
general problem of path planning. The randomized algorithms are based on randomized sampling. Those algorithms select 
some random nodes in the total map and apply own algorithm or method. These algorithms cannot find the optimal path 
compared with the algorithms using graph or tree structure, but they have low computational time. Therefore, they can 
generate path at extremely fast time. The algorithms using this method include randomized potential field, probabilistic 
roadmap (Jerome Barraquand et al., 1996) and RRT (Steven M. Lavalle, 1998) (Pepy, R., 2006) (Chris Urmson and Reid 
Simmons, 2003). Among those, RRT gets many researchers’ attention because it uses fast space exploration method result 
from the Voronoi region. 

In the autonomous robot, path smoothing is as important as path planning. The path planning using randomized 
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algorithms generates a jittered path. The simplest smoothing method for optimizing the jittered path is to connect two 
nodes that have no obstacles between them. However, the smoothed path cannot satisfy real kinematic constraints. Several 
smoothing methods satisfying kinematic constraints include SPP curve smoothing (Nelson, W., 1989) and clothoid curve 
smoothing (Henrie, Joshua and Wilde, Doran, 2007). 

The above path planning and path smoothing are separated at researches, but these cannot be separated when 
these are applied to the real mobile robot. There are some existing research results satisfying both the shortest path and 
robot’s kinematic constraints. However, those researches need instant post-processing causing unpredictable computational 
load, or do not generate the shortest path. 

In this paper, we firstly perform global path planning through the RRT, and propose smoothing of the planned 
path with consideration of the kinematic constraint through the SPP. In Section 2, we introduce to the RRT, SPP and 
proposed method that combine RRT and SPP. In Section 3, we present simulation results and make conclusions. 


METHODS 

Rapidly Exploring Random Tree (RRT) 

The RRT that is one of the path planning methods based on tree structure. It can find nodes and edges from the 
starting point to the arrival point using random node sampling (Steven M. Lavalle, 1998) (Pepy, R., 2006). 


Table 1: The RRT Generation Algorithm 


| GENE RAT E_RRT(x 0 , K, At) 

1 

T. init(x 0 ); 

2 

for k = 1 toKdo 

3 

Xrand = RANDOM _ST ATE (); 

4 

Xnear = N EAREST_N EIGHBOR(x rand ,T); 

5 

u = SELECT _IN PUT (x rand ,x near ); 

6 

X n ew - NEWJSTATE (x near ,U, At); 

7 

T. add_node{x new )) 

8 

T. add_edge(x necL r>x new ,u)', 

9 

Return T 


Table 1 presents a pseudo-code of the RRT generation algorithm. First of all, the algorithm selects random node 
x rancL i n the map. Next, it finds the nearest node x near to the x rand in the treeT, and the input u for arriving to the x rand 
from the x near in the input set U. It calculates real state x new after A fusing the x near and the u. Information of the node 
x new and edge is saved as the children node of the x near . 

RRT has many advantages and the most important advantage is efficiency and fast exploration to the unexplored 
region. This advantage comes from that the RRT has the characteristics of the Voronoi bias. When the pseudo-code of the 
Table 1 is iterated, the probability of the selecting one of the nodes in the tree T to the x near is directly proportional to its 
size of the Voronoi region. The size of the Voronoi region means the size of the empty region and it causes the probability 
of the exploration to the empty region to be bigger. This is the Voronoi bias. 

The node set N RRT generated by the goal biased RRT presents non-optimal path which has jitters without pre- 
smoothing. To reject the jitter, N SM00THED is generated using the simplest smoothing method and new node set N SPP is 
generated using the N SM00THED for the SPP curve smoothing. 
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Table 2: The Pre-Smoothing for the Generation of the SPP Curve 


RRT_PRE_SMOOTHING(N rrt , d factor , Obs) 

1 

N smoothed ■ 

2 

K = length (N RRT ); 

3 

for k = 1 toKdo 

4 

(x 0 ,yo) = N RRTk ; 

5 

C x f>yf ) = N RRTik+1 ; 

6 

Winter ~ j > 

^ factor 

7 

* -Jyf-yo\ 

cp = tan 1 1 I; 

\x f x 0 j 

8 

for l = ltod factor do 

9 

% inter Xq l ’ di n t er COS^Cp^ , 

10 

y inter ~ To T / ' dj nter SUl{cp) ) 

11 

s = COLLISION _CHECK{Obs,x inter ,y inter )) 

12 

if s == TRUE 

13 

N smoothed ■ cidd_node(N RRTk+1 ); 

14 

Return N SM00THED 


Table 2 presents the simplest smoothing method. Distance from the N RRT to the nearest node is divided to 
thed^ actor . After that, the method increases edge’s distance according to the/, and checks the collision, iteratively. If 
collision is detected, the node at the sample k + 1 is saved in theN SM00THED . This smoothing method generates the optimal 
path based on the straight line that does not collide with obstacles. 

Single Polar Polynomial (SPP) Curve 

The SPP curve that is used by the path smoothing method generates continuous curvature path which satisfies 
kinematic constraint of the mobile robot (Nelson, W., 1989) generally, the path generated by path planning consists of the 
line or combination of the line and arc. However, the mobile robot can hardly track the path because real robot has several 
constraints according to the kinematic hardware limit and the path does not consider about them. To obtain the curve 
satisfying the constraints, we can present the SPP curve in the polar coordinate where independent variables can be 
uniformly changed according to the distance on the curve. 



Figure 1: The Radius Change of the SPP Curves 
According to the State of the Rotation Angle 

Figure 1 shows an SPP curve/?. The SPP curve substitutes an arc that has a radius R and a eenterp c , wherer(0)is a 
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variable radius according to the rotation angle#, f (#) is the derivative of the r(#)with respect to#, and f(#) is the second 
derivative of ther(#)with respect to #.r(#), f (#), and f (#)are given in (1) and they have to satisfy a boundary condition 
( 2 ) 


r(#) — CIq + ClRd + $2# ^3# + $4 # + $5# + • • • 

r(#) =i — = + 2a 2 # + 3 a 3 0 2 + 4a 4 # 3 + 5a 5 0 4 h — , (1) 

dO 

dr 1 

r(#) — — - — in 2^ 2 + 6a 3 0 + 12a 4 # 2 + 20 $c# 3 + • • • 
d 2 # 


fr(#) = /?, f(#) = 0, k = 0, at # = 0 
|r(#) = R , r(#) = 0, £ = 0, at # = fi ’ 


Where (pis heading angle, and s is circumference. /c(#) is the curvature for given #and is given as follows: 
( r 2 ( 0 ) + r 2 (^))2 

Substituting (1), (2) to the (3) and calculatinga 0 ~a 5 gives 


R R R 

a 0 = R, a x = 0, a 2 = — , a 3 = , a 4 = — — , a 5 - 0 . 

2 n 2 ju 

So, we can get (5) that present a trajectory of the SPP curve as follows: 
( pR pR> p4 ^ 

m=R 1+ -r+— +— • 

l 2 n ip) 


(4) 


(5) 


Even if we use the above SPP curve, we may not generate proper path that connects a starting point and an arrival 
point when we use 1 -segment path that is one arc or one line. The criterion that considers the number of the segments is to 
determine symmetry or asymmetry let’s define /? as follows: 


P = tan 


-l 


y f ~yp 




If a starting point p 0 = (x 0 ,y 0 , <p 0 ) an d an arrival point p^ = (Xp,y^ ), and /? satisfy 


( 6 ) 


<Po-P = -{<Pf-P )> 


(7) 


Then the two points are said to be symmetric. 

Figure 2 shows 1 -segment and 2-segment paths that connect symmetrical or asymmetrical two points. 



(a) (b) (0 (d) (e) 
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Figure 2: Path (a) and (b) Connecting Asymmetrical Two Points and (c) ~ (e) Connecting Symmetrical Two Points 

The path using 1 -segment consists of the line or one SPP curve, and the path using 2-segment consists of the line + 
one SPP curve, the one SPP curve + line or the two SPP curves. The criterion of the generating each path is referred to the 
paper (Henrie, Joshua and Wilde, Doran, 2007). 


Selection of iV 5PP and Generation of the SPP Curve Via N SPP 



(a) (b) 

Figure 3: (a) The Difference of the Path between N smoothed and N SPP . 
(b) The Difference of the Path According to the Location of theJV 5PP . 


We have to generate new node set N SPP for SPP curve generation using th eN SM00THED . Figure 3 -(a) shows 
generated SPP curves for different node sets given a random starting point and arrival point. Two ellipses denote the 
starting and the arrival point, squares denoteN SM0077/ED , and circles denote Af SPP , respectively. Dashed lines denote paths 
usin gN SM00THED , dash-dot lines denote paths usingiV SPP . Length of the path using N SM00THED is longer than the path 
generated by N RRT . Otherwise, length of the path using N SPP is shorter than the path generated by N RRT . 


Figure 3-(b) shows the different paths according to the location of theiV 5PP . The criterion for generating 
iV 5PP varies with the driving environment and the situation of developer’s concern. The closer N SPP is to th eN SM00THED , the 
less difference those two paths have. That means that when N SPP approaches the N SM00THED , the probability of obstacle 
collision is decreased. On the other hand, when N SPP is far away from the N SM00THED , then path length is shortened. 


In the proposed method, initial iV SPP has the same starting and arrival point &sN SM00THED . Except those two points, 
N SPP corresponds to the midpoint of neighbor nodes in N SM00THED . This method generates shortest path and curves that 
have small curvature for satisfying kinematic constraints. 

Obstacle Collision Detection (Using Geometry) 

The method of Section 2.3 can generate SPP curve satisfying kinematic constraints, but obstacle avoidance 
algorithm has to be applied because the generated path using SPP curve does not have obstacle avoidance mechanism. To 
use it, we need some criteria for the collision detection. Firstly, we’ll check radius size of the SPP curve and an obstacle. 
Secondly, we’ll check possibility of the collision. Finally, we’ll check whether there is collision with an obstacle. For these 
criteria, we’ll consider three assumptions. 
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Firstly, the shape of obstacles is assumed to be a circle and the radius of obstacles is assumed to be bigger than the 
real radius. Circle obstacle has fixed curvature and distance from the center to the surface of the obstacle. These 
characteristics simplify the collision avoidance problem. Secondly, the SPP curve is assumed to be a circle. The SPP curve 
has boundary conditions that distance R from starting point to center is the same as the distance from arrival point to 
center. It means that the SPP curve is assumed to the arc or the circle which has radiusR. Finally, only one SPP curve is 
considered in this method. The path generated by the proposed method consists of a line and a SPP curve, and the line path 
does not collide with obstacles because of the RRT’s characteristics. It means that we can simplify the obstacle avoidance 
problem for all paths as the problem for one SPP curve. Through the above three assumptions, the relation between the SPP 
curve and an obstacle is to be the relation between two circles. 

Three criteria for the obstacle collision detection are like these: Firstly, relation between the radius of an obstacle 
and the SPP curve is compared. Figure 4 is one of the situations where the radius of an obstacle is bigger than the radius of 
the SPP curve. Line paths a and b generated by RRT avoid obstacles due to the RRT’s characteristics. Therefore, there are 
never two intersections. There are no in circles in the in circle sets of the a and b , which have a radius bigger than the SPP 
curve’s. 



Figure 4: When the Radius of an Obstacle is bigger Than the Radius of the SPP Curve 







Figure 5: The Relation between an Obstacle and the Initial SPP Curve, (a) External, 

(b) Externally Tangent, (c) Secant, (d) Internally Tangent, (e) Internal 

Secondly, collision possibility according to the relation between an obstacle and the SPP curve is checked. Figure 
5 presents the relation between an obstacle and initial SPP curve. Dash-double dot line denotes real SPP curve, black circle 
with radius R denotes initial SPP curve, and red circle denotes an obstacle. Through the Figure 5, secant situation is 
considered where obstacle collision occurs. By the assumption, external and externally tangent situation do not cause 
obstacle collision, but real SPP curve has possibility of the obstacle collision at external and externally tangent situation 
because its radius is bigger than or equal to the assumed circle’s radius. 

Finally, a criterion for checking real collision should be made according to the above collision detection. The each 


Impact Factor (JCC): 3.6986 


NAAS Rating: 3.06 


Fast and Kinematic Constraint-Satisfying Path Planning with Obstacle Avoidance 


23 


region A on the Figure 6 denotes an obstacle that collides with the real SPP curve. Therefore, when the relation of two 
circles is external or externally tangent state, the collision is detected if the center of an obstacle is in the region A 
consisting of the linesa, b and c. In the secant situation, the collision is detected if the center of an obstacle is in the region 
A consisting of the linesa, b, c and d. 




(a) 

Figure 6: The Region of the Collision between an Obstacle and the Initial SPP Curve 
(a) External and Externally Tangent, (b) Secant 

Obstacle Avoidance Algorithm (Using Geometry) 

Through the above section 2.4, we can detect obstacle collision. If there is a collision, new SPP curve has to be 
generated having radiusTU. At this moment, the new SPP curve having the optimal radius R' has to be internally tangent 
state to an obstacle. Here, the center of the initial SPP curve is (x, y) and the radius of that is R , the center of the new SPP 
curve is {pc', y') and the radius of that is R'. 



Figure 7: The Optimal SPP Curve that does not Collide with an Obstacle 

R =/sin<T=( A /(x'-x ,) 2 +(>’’->’ ) 2 )sin(T 


R - r=d= ^- X( J +{y <- y( J 


R = 


(p s +(lcC6<j)’Uf)-p 


( 8 ) 


where p obs denotes center of an obstacle, p s denotes one of the nodesin the N< 


SMOOTHED 


, and u 0 , Uf are unit 
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direction vectors of the start node and end node. The start node and end node are calculated using(x',y') and R ' that are 
generated by simultaneous equations (8). Through the above, we can generate an optimal SPP curve that is internally 
tangent state to an obstacle. 

RESULTS 

Simulation 

The first simulation is performed with the starting point (0, 0) and the arrival point (400,400). The centers of four 
obstacles are (180,180), (310,230), (150,350), (300,350) and the radii of those are 50. The probability variablep, which is 
same with the probability of x rancL = Xp is 0.5. For the simulation, we use a PC with a processor Intel Core i7-4770 
3.40GHz with 8GB RAM memory 



Figure 8: The Difference between the Proposed 
Method and the Original RRT Path 

Figure 8 is simulation result. Large circles denote obstacles. Dotted lines denote path obtained from simple 
smoothing. Two paths are same on the straight line part, but the proposed method satisfies kinematic constraints on the 
curved part. 

In the second simulation, we check the performance of the proposed obstacle avoidance algorithm. The obstacle 
with a center at (180,180) is moved to (188,203) for obstacle collision. In Figure 9-(b) and (c), red circle consisting of 
dashed line denotes the SPP curve represented by circle. An internally tangent circle that means an optimal SPP curve is 
perfectly generated in this simulation. 





(C) 

Figure 9: (a) The Path Generation not using the Proposed Obstacle Avoidance Algorithm 
(b) The Path Generation using the Proposed Obstacle Avoidance Algorithm 
(c) The path Generation using the Proposed Obstacle Avoidance Algorithm (Zoom-In) 
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In the third simulation, we measure average execution time obtained for different goal bias probability to check 
the performance of the proposed method. Figure 10 shows the execution time graph for 500 iterations withp = 0.20. 
Average execution time is 0.5763s, and deviation is somewhat large because RRT uses random sampling. RRT execution 
time is 0.5451s and execution time for SPP smoothing and obstacle avoidance algorithm is 5.5% of the total execution 
time. It is negligible amount compared with total execution time. Table 3 is total records of the execution time. The relation 
between the probability p and the execution time is nonlinear. For better performance of the proposed method, the most 
proper probability p has to be selected depending on the experiment environment. 



0 50 100 150 200 250 300 350 400 450 500 


Figure 10: The Execution Time Graph when the Proposed 
Method is iterated 500 Times (p = 0. 20) 


Table 3: The Total Execution Time for Different Goal Bias Probability p 


V 

RRT (s) 

SPP Smoothing (s) 

Total (s) 

0.10 

0.6303 

0.0294 

0.6597 

0.20 

0.5451 

0.0312 

0.5763 

0.25 

0.5088 

0.0346 

0.5435 

0.30 

0.5299 

0.0338 

0.5637 

0.35 

0.5478 

0.0343 

0.5821 

0.40 

0.5272 

0.0352 

0.5623 

0.45 

0.5833 

0.0356 

0.6189 

0.50 

0.6463 

0.0349 

0.6812 


Finally, we compare performance of the A* algorithm, which is typical path planning algorithm, with the 
proposed method on the final simulation. The program included in the report (Alex Andrien, 2012), is used for the 
simulation of the A* algorithm. Figure 11 shows two A* algorithms. Execution time of the A* Manhattan is 2.971s, and 
execution time of the A* straight is 397.784s. This result shows that the execution time of the A* algorithms are too long. 
Furthermore, the algorithms can’t satisfy the kinematic constraints. Therefore, A* algorithm can be hardly applied to real 
experiment. 
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(a) (b) 


Figure 11: (a) The Path Planning Using the A* Algorithm (Manhattan Distance). 

(b) The Path Planning Using the A* Algorithm (Straight Distance) 

CONCLUSIONS 

General path planning has lots of computational loads, or consists of the only straight lines, and it causes some 
limitation that the robot cannot follow the planned path directly. In this paper, we propose the method using the RRT that 
shows better performance in terms of the computational load and speed compared with other general path planning, and the 
SPP that can smooth the path so that it can satisfy kinematic constraints. Through the simulation, the proposed method 
shows better performance in the real experiment compared with RRT only. Furthermore, computational speed is faster than 
the typical path planning. Through the proposed method, we can expect to get the better results satisfying the kinematic 
constraints of the robot when it is applied in the experiment. 
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